#ifndef FRAME_H
#define FRAME_H
#include <opencv2/opencv.hpp>
#include "common.h"
#include "MapPoint.h"
#include "KeyFrame.h"

#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64


class MapPoint;
class KeyFrame;

class Frame
{
public:
	Frame();

	~Frame();

	Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, cv::Mat &K, cv::Mat &distCoef, const float &bf, float thDepth = 3.85f);

	void UndistortKeyPoints();

	void ComputeStereoMatches();

	void ComputeStereoMatches_orb();

	cv::Mat GetCameraCenter();

	// Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
	cv::Mat UnprojectStereo(const int &i);

	// Set the camera pose.
	void SetPose(cv::Mat Tcw);

	// Computes rotation, translation and camera center matrices from the camera pose.
	void UpdatePoseMatrices();

	//Check if a MapPoint is in the frustum of the camera
	// and fill variables of the MapPoint to be used by the tracking
	bool isInFrustum(MapPoint* pMP, float viewingCosLimit);

	// Computes image bounds for the undistorted image (called in the constructor).
	void ComputeImageBounds(const cv::Mat &imLeft);

	void AssignFeaturesToGrid();

	// Compute the cell of a keypoint (return false if outside the grid)
	bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);

	std::vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r);

public:
	long unsigned int m_frameId;
	// Frame timestamp.
    double m_TimeStamp;

	//zoom ratio
	float m_zoomRatio;
	
	cv::Mat m_K;
	cv::Mat m_DistCoef;
	// Stereo baseline multiplied by fx.
    float m_baselineFx;

	// Stereo baseline in meters.
    float m_baseline;

	static float m_fx;
    static float m_fy;
    static float m_cx;
    static float m_cy;
    static float m_invfx;
    static float m_invfy;

	// Threshold close/far points. Close points are inserted from 1 view.
    // Far points are inserted as in the monocular case from 2 views.
    float m_ThDepth;

	// MapPoints associated to keypoints, NULL pointer if no association.
    std::vector<MapPoint*> m_vectorMapPoints;

	// Flag to identify outlier associations.
    std::vector<bool> m_vectorOutlier;

	// Corresponding stereo coordinate and depth for each keypoint.
    std::vector<float> m_vectorURight;
    std::vector<float> m_vectorDepth;

	// Number of KeyPoints.
    int m_keyPointsNumber;
	std::vector<cv::KeyPoint> m_vectorKeyPointsLeft, m_vectorKeyPointsRight;
	std::vector<cv::KeyPoint> m_vectorKeyPointsLeftUn;

	cv::Mat m_DescriptorsLeft, m_DescriptorsRight;

	// Camera pose.
    cv::Mat m_RTcw;

	Vector3f m_position;

	//x is roll, y is pitch, z yaw
	Vector3f m_attitude;

	// Reference Keyframe.
    KeyFrame* m_ReferenceKF;

	// Undistorted Image Bounds (computed once).
	static float m_MinX;
	static float m_MaxX;
	static float m_MinY;
	static float m_MaxY;

	// Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
    static float m_GridElementWidthInv;
    static float m_GridElementHeightInv;
    std::vector<std::size_t> m_Grid[FRAME_GRID_COLS][FRAME_GRID_ROWS];

private:
	// Rotation, translation and camera center
	//camera to world rotation
	cv::Mat m_Rcw;

	//camera to world translation
    cv::Mat m_tcw;

	//world to camera rotation
    cv::Mat m_Rwc;

	//camera center
    cv::Mat m_Ow; //==m_twc
};

#endif //FRAME_H
